#include "driver_Mobvoi_includes.h"
#include "app_Mobvoi_includes.h"
#include "Mobvoi_external.h"
#include <tgmath.h> //add by zjw 131008

#if (CONFIG_AUD_EQS_SUPPORT == 1) //by Mobvoi zb
//EQ 1
#define TAB1_TotalEQnum 5
#define TAB1_EQGAIN 3

#define TAB1_EQ0A0 -1972996
#define TAB1_EQ0A1 936999
#define TAB1_EQ0B0 1074196
#define TAB1_EQ0B1 -1966879
#define TAB1_EQ0B2 917495

#define TAB1_EQ1A0 -1675702
#define TAB1_EQ1A1 738322
#define TAB1_EQ1B0 1062469
#define TAB1_EQ1B1 -1668196
#define TAB1_EQ1B2 731935

#define TAB1_EQ2A0 -1216723
#define TAB1_EQ2A1 538819
#define TAB1_EQ2B0 983084
#define TAB1_EQ2B1 -1269332
#define TAB1_EQ2B2 551701

#define TAB1_EQ3A0 -861351
#define TAB1_EQ3A1 440959
#define TAB1_EQ3B0 1234504
#define TAB1_EQ3B1 -660147
#define TAB1_EQ3B2 456235

#define TAB1_EQ4A0 112655
#define TAB1_EQ4A1 -351039
#define TAB1_EQ4B0 850529
#define TAB1_EQ4B1 149624
#define TAB1_EQ4B2 -286804

//EQ2
#define TAB2_TotalEQnum 5
#define TAB2_EQGAIN -2

#define TAB2_EQ0A0 -2033449
#define TAB2_EQ0A1 988428
#define TAB2_EQ0B0 1045285
#define TAB2_EQ0B1 -2033798
#define TAB2_EQ0B2 991369

#define TAB2_EQ1A0 -1764286
#define TAB2_EQ1A1 789963
#define TAB2_EQ1B0 1110313
#define TAB2_EQ1B1 -1733071
#define TAB2_EQ1B2 759440

#define TAB2_EQ2A0 -1312606
#define TAB2_EQ2A1 572732
#define TAB2_EQ2B0 1266296
#define TAB2_EQ2B1 -1117406
#define TAB2_EQ2B2 550212

#define TAB2_EQ3A0 -611466
#define TAB2_EQ3A1 394830
#define TAB2_EQ3B0 1074937
#define TAB2_EQ3B1 -581716
#define TAB2_EQ3B2 398218

#define TAB2_EQ4A0 602306
#define TAB2_EQ4A1 393461
#define TAB2_EQ4B0 782275
#define TAB2_EQ4B1 210400
#define TAB2_EQ4B2 267856

//EQ3
#define TAB3_TotalEQnum 5
#define TAB3_EQGAIN -3

#define TAB3_EQ0A0 -2082978
#define TAB3_EQ0A1 1034453
#define TAB3_EQ0B0 1051646
#define TAB3_EQ0B1 -2082951
#define TAB3_EQ0B2 1031409

#define TAB3_EQ1A0 -2043965
#define TAB3_EQ1A1 996660
#define TAB3_EQ1B0 1044892
#define TAB3_EQ1B1 -2044129
#define TAB3_EQ1B2 1000180

#define TAB3_EQ2A0 -1670349
#define TAB3_EQ2A1 667006
#define TAB3_EQ2B0 1113044
#define TAB3_EQ2B1 -1654896
#define TAB3_EQ2B2 617992

#define TAB3_EQ3A0 494878
#define TAB3_EQ3A1 -65555
#define TAB3_EQ3B0 1036792
#define TAB3_EQ3B1 481467
#define TAB3_EQ3B2 -67182

#define TAB3_EQ4A0 602306
#define TAB3_EQ4A1 393461
#define TAB3_EQ4B0 782275
#define TAB3_EQ4B1 210400
#define TAB3_EQ4B2 267856

const static int32 eq1_tab_arr[TAB1_TotalEQnum][5] =
    {{-TAB1_EQ0A0, -TAB1_EQ0A1, TAB1_EQ0B0, TAB1_EQ0B1, TAB1_EQ0B2},
     {-TAB1_EQ1A0, -TAB1_EQ1A1, TAB1_EQ1B0, TAB1_EQ1B1, TAB1_EQ1B2},
     {-TAB1_EQ2A0, -TAB1_EQ2A1, TAB1_EQ2B0, TAB1_EQ2B1, TAB1_EQ2B2},
     {-TAB1_EQ3A0, -TAB1_EQ3A1, TAB1_EQ3B0, TAB1_EQ3B1, TAB1_EQ3B2},
     {-TAB1_EQ4A0, -TAB1_EQ4A1, TAB1_EQ4B0, TAB1_EQ4B1, TAB1_EQ4B2}};
const static int32 eq2_tab_arr[TAB2_TotalEQnum][5] =
    {{-TAB2_EQ0A0, -TAB2_EQ0A1, TAB2_EQ0B0, TAB2_EQ0B1, TAB2_EQ0B2},
     {-TAB2_EQ1A0, -TAB2_EQ1A1, TAB2_EQ1B0, TAB2_EQ1B1, TAB2_EQ1B2},
     {-TAB2_EQ2A0, -TAB2_EQ2A1, TAB2_EQ2B0, TAB2_EQ2B1, TAB2_EQ2B2},
     {-TAB2_EQ3A0, -TAB2_EQ3A1, TAB2_EQ3B0, TAB2_EQ3B1, TAB2_EQ3B2},
     {-TAB2_EQ4A0, -TAB2_EQ4A1, TAB2_EQ4B0, TAB2_EQ4B1, TAB2_EQ4B2}};
const static int32 eq3_tab_arr[TAB3_TotalEQnum][5] =
    {{-TAB3_EQ0A0, -TAB3_EQ0A1, TAB3_EQ0B0, TAB3_EQ0B1, TAB3_EQ0B2},
     {-TAB3_EQ1A0, -TAB3_EQ1A1, TAB3_EQ1B0, TAB3_EQ1B1, TAB3_EQ1B2},
     {-TAB3_EQ2A0, -TAB3_EQ2A1, TAB3_EQ2B0, TAB3_EQ2B1, TAB3_EQ2B2},
     {-TAB3_EQ3A0, -TAB3_EQ3A1, TAB3_EQ3B0, TAB3_EQ3B1, TAB3_EQ3B2},
     {-TAB3_EQ4A0, -TAB3_EQ4A1, TAB3_EQ4B0, TAB3_EQ4B1, TAB3_EQ4B2}};
app_aud_eq_t app_aud_eq[APP_EQ_NUM];

void app_aud_eq_init(void)
{
    uint8_t i;
    uint8_t j;
    for (i = 0; i < APP_EQ_NUM; i++)
    {
        app_aud_eq[i].eq_enable = 0;
    }

    for (i = 0; i < APP_EQ_NUM; i++)
    {
        if (i == 0)
        {
            app_aud_eq[0].eq_gain = TAB1_EQGAIN;

            for (j = 0; j < TAB1_TotalEQnum; j++)
            {
                app_aud_eq[0].eq_enable |= 1 << j;
                app_aud_eq[i].eq_para[j].a[0] = eq1_tab_arr[j][0];
                app_aud_eq[i].eq_para[j].a[1] = eq1_tab_arr[j][1];
                app_aud_eq[i].eq_para[j].b[0] = eq1_tab_arr[j][2];
                app_aud_eq[i].eq_para[j].b[1] = eq1_tab_arr[j][3];
                app_aud_eq[i].eq_para[j].b[2] = eq1_tab_arr[j][4];
            }
        }
        else if (i == 1)
        {
            app_aud_eq[1].eq_gain = TAB2_EQGAIN;

            for (j = 0; j < TAB2_TotalEQnum; j++)
            {
                app_aud_eq[1].eq_enable |= 1 << j;
                app_aud_eq[i].eq_para[j].a[0] = eq2_tab_arr[j][0];
                app_aud_eq[i].eq_para[j].a[1] = eq2_tab_arr[j][1];
                app_aud_eq[i].eq_para[j].b[0] = eq2_tab_arr[j][2];
                app_aud_eq[i].eq_para[j].b[1] = eq2_tab_arr[j][3];
                app_aud_eq[i].eq_para[j].b[2] = eq2_tab_arr[j][4];
            }
        }
        else if (i == 2)
        {
            app_aud_eq[2].eq_gain = TAB3_EQGAIN;
            for (j = 0; j < TAB3_TotalEQnum; j++)
            {
                app_aud_eq[2].eq_enable |= 1 << j;
                app_aud_eq[i].eq_para[j].a[0] = eq3_tab_arr[j][0];
                app_aud_eq[i].eq_para[j].a[1] = eq3_tab_arr[j][1];
                app_aud_eq[i].eq_para[j].b[0] = eq3_tab_arr[j][2];
                app_aud_eq[i].eq_para[j].b[1] = eq3_tab_arr[j][3];
                app_aud_eq[i].eq_para[j].b[2] = eq3_tab_arr[j][4];
            }
        }
    }
}

void app_eq_pram_sel(void)
{
    app_env_handle_t env_h = app_env_get_handle();
    if (env_h->env_data.eq_lang_sel < APP_EQ_NUM)
    {
        memcpy(&env_h->env_cfg.aud_eq.eq_enable, &app_aud_eq[env_h->env_data.eq_lang_sel].eq_enable, sizeof(app_aud_eq_t));
    }
    else
    {
        env_h->env_data.eq_lang_sel = 0;
        memcpy(&env_h->env_cfg.aud_eq.eq_enable, &app_aud_eq[env_h->env_data.eq_lang_sel].eq_enable, sizeof(app_aud_eq_t));
    }
}
void app_online_eq_gain_enable(uint16_t enable, int16_t gain)
{
    extern aud_pre_equ_t aud_pre_eqe;
    app_env_handle_t env_h = app_env_get_handle();
    env_h->env_cfg.aud_eq.eq_enable = enable;
    env_h->env_cfg.aud_eq.eq_gain = gain;
    float V0 = powf(10, (float)env_h->env_cfg.aud_eq.eq_gain / 20);
    aud_pre_eqe.globle_gain = (uint32)((float)0x4000 * V0);
}
void eq_onlin_set(void)
{
#if (CONFIG_PRE_EQ == 1)
    app_env_handle_t env_h = app_env_get_handle();
    extern aud_pre_equ_t aud_pre_eqe;
    extern SAMPLE_ALIGN aud_pre_equ_para_t tbl_eq_coef[];
    app_eq_pram_sel();
    aud_pre_eqe.online_flag = 0;
    aud_pre_eqe.totle_EQ = 0;
    uint16 i;
    for (i = 0; i < CON_AUD_EQ_BANDS; i++)
    {
        if ((1 << i) & env_h->env_cfg.aud_eq.eq_enable)
        {
            tbl_eq_coef[i].a[0] = env_h->env_cfg.aud_eq.eq_para[i].a[0];
            tbl_eq_coef[i].a[1] = env_h->env_cfg.aud_eq.eq_para[i].a[1];
            tbl_eq_coef[i].b[0] = env_h->env_cfg.aud_eq.eq_para[i].b[0];
            tbl_eq_coef[i].b[1] = env_h->env_cfg.aud_eq.eq_para[i].b[1];
            tbl_eq_coef[i].b[2] = env_h->env_cfg.aud_eq.eq_para[i].b[2];
            aud_pre_eqe.totle_EQ++;
            aud_pre_eqe.online_flag = 0x5a;
        }
        else
        {
            tbl_eq_coef[i].a[0] = 0;
            tbl_eq_coef[i].a[1] = 0;
            tbl_eq_coef[i].b[0] = 0x100000;
            tbl_eq_coef[i].b[1] = 0;
            tbl_eq_coef[i].b[2] = 0;
        }
    }
#endif
}

#endif
#ifdef CONFIG_APP_EQUANLIZER
#define AUD_EQU_NUM 5 //music 5 and aec 2
#define AUD_EQU_HF_START 5
#define AUD_EQU_HF_STOP 6

static aud_equ_t aud_equ[AUD_EQU_NUM + 2];
static int equ_enable_count = 0;

#if (CONFIG_PRE_EQ == 1)
extern aud_pre_equ_t aud_pre_eqe;
extern void func_sw_eq(int *input, int *output, int len, int eq_cnt, uint32_t index); //len: 1=L+R=4bytes
#endif
#if (CONFIG_HFP_SPK_EQ == 1)
extern aud_pre_equ_t hfp_spk_eqe;
extern void func_sw_spk_eq(int *input, int *output, int len, int eq_cnt, uint32_t index); //len: 1=L+R=4bytes
#endif
#if (CONIFG_HFP_MIC_EQ == 1)
extern aud_pre_equ_t hfp_mic_eqe;
extern void func_sw_mic_eq(int *input, int *output, int len, int eq_cnt, uint32_t index); //len: 1=L+R=4bytes
#endif

static int16_t aud_equalizer_one_sample(int index, int lr_mode, int16_t sample)
{
    int out_sample = 0;
    int out_sample1 = 0;

    aud_equ[index].x[lr_mode][0] = aud_equ[index].x[lr_mode][1];
    aud_equ[index].x[lr_mode][1] = aud_equ[index].x[lr_mode][2];
    aud_equ[index].x[lr_mode][2] = sample / 2;

    out_sample += (int)aud_equ[index].b[0] * aud_equ[index].x[lr_mode][2];
    out_sample += (int)aud_equ[index].b[1] * aud_equ[index].x[lr_mode][1];
    out_sample += (int)aud_equ[index].b[2] * aud_equ[index].x[lr_mode][0];
    out_sample -= (int)aud_equ[index].a[0] * aud_equ[index].y[lr_mode][1];
    out_sample -= (int)aud_equ[index].a[1] * aud_equ[index].y[lr_mode][0];

    out_sample = out_sample / 4096;

    aud_equ[index].y[lr_mode][0] = aud_equ[index].y[lr_mode][1];
    if (out_sample > 32767)
        aud_equ[index].y[lr_mode][1] = 32767;
    else if (out_sample < -32767)
        aud_equ[index].y[lr_mode][1] = -32767;
    else
        aud_equ[index].y[lr_mode][1] = out_sample;

    out_sample1 = aud_equ[index].y[lr_mode][1] * 2;
    if (out_sample1 > 32767)
        out_sample1 = 32767;
    else if (out_sample1 < -32767)
        out_sample1 = -32767;

    return out_sample1;
}

int aud_apply_equalizer(int index, aud_equ_conf_t *conf)
{
#if 0
    float pi = 3.1415926f;
    float K, V0;
    float t1, t2, t4, t5;
    float Q;

	if( conf->flag_type )  //peak
	{
		Q = (float)conf->fc/conf->bandwidth;
		K = tanf(pi*conf->fc/conf->fs);
		t1 = K*K;

		if ( conf->gain > 0 )
		{
			V0 = powf( 10, (float)conf->gain/20 );
			t4 = 1/Q*K;
			aud_equ[index].b[0] = (int)(4096/(1+t4+t1)*(1+V0*t4+t1));
			aud_equ[index].b[1] = (int)(4096/(1+t4+t1)*(2*(t1-1)));
			aud_equ[index].b[2] = (int)(4096/(1+t4+t1)*(1-V0*t4+t1));
			aud_equ[index].a[0] = (int)(4096/(1+t4+t1)*(2*(t1-1)));
			aud_equ[index].a[1] = (int)(4096/(1+t4+t1)*(1-t4+t1));
		}
		else
		{
			V0 = powf( 10, (float)(-conf->gain)/20 );
			t4 = 1/Q*K;
			aud_equ[index].b[0] = (int)(4096/(1+V0*t4+t1)*(1+t4+t1));
			aud_equ[index].b[1] = (int)(4096/(1+V0*t4+t1)*(2*(t1-1)));
			aud_equ[index].b[2] = (int)(4096/(1+V0*t4+t1)*(1-t4+t1));
			aud_equ[index].a[0] = (int)(4096/(1+V0*t4+t1)*(2*(t1-1)));
			aud_equ[index].a[1] = (int)(4096/(1+V0*t4+t1)*(1-V0*t4+t1));
		}
	}
	else  //shelving
	{
		K = tanf(pi*conf->fc/conf->fs);
		t1 = K*K;
		t5 = sqrtf(2)*K; // sqrt(2)*K

		if( conf->flag_low_high == 0 ) // low
		{
			if( conf->gain > 0 )
			{
				V0 = powf( 10, (float)conf->gain/20 );
				t4 = sqrtf(2*V0)*K; // sqrt(2*V0)*K
				aud_equ[index].b[0] = (int)(4096/(1+t5+t1)*( 1 + t4+V0*t1));
				aud_equ[index].b[1] = (int)(4096/(1+t5+t1)*( 2 *(V0*t1-1)));
				aud_equ[index].b[2] = (int)(4096/(1+t5+t1)*( 1-t4+V0*t1));
				aud_equ[index].a[0] = (int)(4096/(1+t5+t1)*( 2*(t1-1)));
				aud_equ[index].a[1] = (int)(4096/(1+t5+t1)*( 1-t5+t1));
			}
			else
			{
				V0 = powf( 10, (float)(-conf->gain)/20 );
				t4 = sqrtf(2*V0)*K; // sqrt(2*V0)*K
				aud_equ[index].b[0] = (int)(4096/(1+t4+V0*t1)*(1+t5+t1));
				aud_equ[index].b[1] = (int)(4096/(1+t4+V0*t1)*(2*(t1-1)));
				aud_equ[index].b[2] = (int)(4096/(1+t4+V0*t1)*(1-t5+t1));
				aud_equ[index].a[0] = (int)(4096/(1+t4+V0*t1)*(2*(V0*t1-1)));
				aud_equ[index].a[1] = (int)(4096/(1+t4+V0*t1)*(1-t4+V0*t1));
			}
		}
		else
		{
			if( conf->gain > 0 )
			{
				V0 = powf( 10, (float)conf->gain/20 );
				t4 = sqrtf(2*V0)*K; // sqrt(2*V0)*K
				aud_equ[index].b[0] = (int)(4096/(1+t5+t1)*( V0+t4+t1));
				aud_equ[index].b[1] = (int)(4096/(1+t5+t1)*( 2*(t1-V0)));
				aud_equ[index].b[2] = (int)(4096/(1+t5+t1)*(V0-t4+t1));
				aud_equ[index].a[0] = (int)(4096/(1+t5+t1)*( 2*(t1-1)));
				aud_equ[index].a[1] = (int)(4096/(1+t5+t1)*( 1-t5+t1));
			}
			else
			{
				V0 = powf( 10, (float)(-conf->gain)/20 );
				t2 = sqrtf(2/V0)*K;
				t4 = sqrtf(2*V0)*K; // sqrt(2*V0)*K
				aud_equ[index].b[0] = (int)(4096/(V0+t4+t1)*(1+t5+t1));
				aud_equ[index].b[1] = (int)(4096/(V0+t4+t1)*(2*(t1-1)));
				aud_equ[index].b[2] = (int)(4096/(V0+t4+t1)*(1-t5+t1));
				aud_equ[index].a[0] = (int)(4096/(1+t2+t1/V0)*(2*(t1/V0-1)));
				aud_equ[index].a[1] = (int)(4096/(1+t2+t1/V0)*(1-t2+t1/V0));
			}
		}
	}
#endif
    return 0;
}

void aud_euqalizer_enable(int index, uint8_t enable)
{
    aud_equ[index].flag_enable = enable;

    memset((uint8_t *)&aud_equ[index].x[0][0], 0, sizeof(aud_equ[index].x));
    memset((uint8_t *)&aud_equ[index].y[0][0], 0, sizeof(aud_equ[index].y));

    if (index < AUD_EQU_NUM)
    {
        equ_enable_count = aud_equ[0].flag_enable + aud_equ[1].flag_enable + aud_equ[2].flag_enable + aud_equ[3].flag_enable + aud_equ[4].flag_enable;

        /* if( app_bt_flag1_get( APP_FLAG_MUSIC_PLAY ) ) */
        /* { */
        /*     if( a2dp_get_freq() == 44100 ) */
        /*     { */
        /*         if( equ_enable_count >= 1 ) */
        /*             ; */
        /*     } */
        /*     else if( a2dp_get_freq() == 48000 ) */
        /*     { */
        /*         if( equ_enable_count >= 1 ) */
        /*             ; */
        /*     } */
        /* } */
    }
}

int app_equalizer_get_enable_count(void)
{
    return equ_enable_count;
}

void app_equalizer_recover_enable(void)
{
    //os_printf("---app_equalizer_recover_enable()\r\n");
}

void app_equalizer_init(void)
{
    int i;
    for (i = 0; i < AUD_EQU_NUM; i++)
    {
        memset((uint8_t *)&aud_equ[i], 0, sizeof(aud_equ_t));
        aud_equ[i].b[0] = 0x4000 << 6;
    }
}
void app_equalizer_a2dp_apply(void)
{
}
void app_equalizer_hfp_apply(void)
{
#if 0
    int i;
    app_eq_para_t eq_para;
    memset( (uint8_t *)&eq_para, 0, sizeof(app_eq_para_t));
    eq_para.b[0] = 0x4000<<6;
    for( i = 0; i < AUDIO_MAX_FILT_NUM; i++ )
    {
        aud_filt_conf( i, &eq_para );
    }
    return;
#endif
}

void app_equalizer_calc(int16_t samplel, int16_t sampler, int16_t *outl, int16_t *outr)
{
    int k;

    for (k = 0; k < AUD_EQU_NUM; k++)
    {
        if (aud_equ[k].flag_enable == 1)
        {
            *outl = aud_equalizer_one_sample(k, 0, samplel);
            *outr = aud_equalizer_one_sample(k, 1, sampler);
            samplel = *outl;
            sampler = *outr;
        }
    }

    return;
}

int16_t app_equalizer_calc_hfp(int16_t sample)
{
    int k;
    int16_t sample_out = sample;

    for (k = AUD_EQU_HF_START; k <= AUD_EQU_HF_STOP; k++)
    {
        if (aud_equ[k].flag_enable == 1)
        {
            sample_out = aud_equalizer_one_sample(k, 0, sample_out);
        }
    }

    return sample_out;
}

#if 0
void app_equalizer_a2dp_init( app_eq_para_t * app_a2dp_equ )
{
#if 0
    int i;
    int count = 0;

    for( i = 0; i < AUD_EQU_NUM; i++ )
    {
        memcpy( aud_equ[i].a, app_a2dp_equ[i].a, sizeof(aud_equ[i].a)+sizeof(aud_equ[i].b));
        aud_euqalizer_enable( i, app_a2dp_equ[i].flag );
        if( app_a2dp_equ[i].flag == 1 )
            count++;
        else
            app_a2dp_equ[i].flag = 0;
    }

    equ_enable_count = count;
#endif
}
#endif

void app_equalizer_show(void)
{
    int i;

    for (i = 0; i < 7; i++)
    {
        os_printf("equ index: %d\r\n", i);
        os_printf("a[0]: %d, a[1]: %d, b[0]: %d, b[1]: %d, b[2]: %d\r\n",
                  aud_equ[i].a[0], aud_equ[i].a[1], aud_equ[i].b[0], aud_equ[i].b[1],
                  aud_equ[i].b[2]);
        os_printf("equ enable: %d\r\n", aud_equ[i].flag_enable);
    }
}

#if (CONFIG_PRE_EQ == 1)
void pre_eq_proc(int32_t *buff, uint16_t size)
{
    int32_t p;
    int32_t *ptr;
    int64 cal_tmp = 0;

    ptr = buff;
    if ((aud_pre_eqe.online_flag == 0x5a) || (aud_pre_eqe.online_flag == 1))
    {
        for (p = 0; p < size; p++)
        {
            cal_tmp = (int64)ptr[p] * aud_pre_eqe.globle_gain;
            cal_tmp >>= 14;
            ptr[p] = (int32_t)cal_tmp;
        }
        func_sw_eq((int *)ptr, (int *)ptr, size >> 1, aud_pre_eqe.totle_EQ, 0);
        for (p = 0; p < size; p++)
        {
            if (ptr[p] > 8388607)
                ptr[p] = 8388607;
            else if (ptr[p] < -8388607)
                ptr[p] = -8388607;
        }
    }
}
#endif

#if (CONFIG_HFP_SPK_EQ == 1)
void hfp_spk_eq_proc(uint8_t *input, uint16_t size)
{
    int32_t data_tmp[128], p;
    int16_t *ptr;

    if (size <= 0 || size > 256)
        return;
    ptr = (int16_t *)input;
    if ((hfp_spk_eqe.online_flag == 0x5a) || (hfp_spk_eqe.online_flag == 1))
    {
        for (p = 0; p < size / 2; p++)
        {
            data_tmp[p] = ptr[p] * hfp_spk_eqe.globle_gain;
        }
        func_sw_spk_eq((int *)data_tmp, (int *)data_tmp, size / 2, hfp_spk_eqe.totle_EQ, 0);
        for (p = 0; p < size / 2; p++)
        {
            data_tmp[p] >>= 14;
            if (data_tmp[p] > 32767)
                ptr[p] = 32767;
            else if (data_tmp[p] < -32767)
                ptr[p] = -32767;
            else
                ptr[p] = data_tmp[p];
        }
    }
}
#endif

#if (CONIFG_HFP_MIC_EQ == 1)
void hfp_mic_eq_proc(uint8_t *input, uint16_t size)
{
    int32_t data_tmp[128], p;
    int16_t *ptr;

    if (size <= 0 || size > 256)
        return;
    ptr = (int16_t *)input;
    if ((hfp_mic_eqe.online_flag == 0x5a) || (hfp_mic_eqe.online_flag == 1))
    {
        for (p = 0; p < size / 2; p++)
        {
            data_tmp[p] = ptr[p] * hfp_mic_eqe.globle_gain;
        }
        func_sw_mic_eq((int *)data_tmp, (int *)data_tmp, size / 2, hfp_mic_eqe.totle_EQ, 0);
        for (p = 0; p < size / 2; p++)
        {
            data_tmp[p] >>= 14;
            if (data_tmp[p] > 32767)
                ptr[p] = 32767;
            else if (data_tmp[p] < -32767)
                ptr[p] = -32767;
            else
                ptr[p] = data_tmp[p];
        }
    }
}
#endif
#endif
